Safe, Optimal, Real-Time Trajectory Planning With a Parallel Constrained Bernstein Algorithm

To move while using new sensor information, mobile robots use receding-horizon planning, executing a short plan computing one. A should have dynamic feasibility (obeying robot's dynamics and avoiding obstacles), liveness (planning frequently enough to complete tasks), optimality (minimizing, e.g., distance goal). Reachability-based trajectory design (RTD) is method generate provably dynamically feasible plans in real time by solving polynomial optimization program (POP) each planning iteration. However, RTD uses derivative-based solver, which may converge local minima that impact optimality. This article proposes parallel constrained Bernstein algorithm (PCBA) branch-and-bound optimally solve RTD's POP at runtime; the resulting optimal planner called RTD*. The specific contributions of this are PCBA implementation, proofs PCBA's bounded memory usage, comparison with state-of-the-art solvers, demonstration PCBA/RTD* on hardware. RTD* shows better than dozens environments random obstacles.